#ifndef _ARMOR_
#define _ARMOR_

#define setting_time 0.08
#include <iostream>
#include <string>
#include <fstream>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <algorithm>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <memory>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/types_c.h>
#include "DataStruct.h"
#include "LightBlob.h"

using namespace std;
using namespace cv;

class Armor;
using Armor_ptr = std::shared_ptr<Armor>;

struct light_blobs
{
    Blob_ptr left;
    Blob_ptr right;
};

struct ResultPnP
{
    double yaw;
    double picth;
    double roll;
    Mat tran_vec;
    double distance;
};

enum class ArmorType
{
    Small = 0,
    Big = 1
};

class Armor
{
public:
    const vector<Point3f> small_armor{{-67.5, 26.5, -7},
                                      {-67.5, -26.5, 7},
                                      {67.5, -26.5, 7},
                                      {67.5, 26.5, -7}};
    const vector<Point3f> big_armor{{-113, 26.5, -7},
                                    {-113, -26.5, 7},
                                    {113, -26.5, 7},
                                    {113, 26.5, -7}};
    light_blobs blobs;
    ResultPnP pnp_data;
    short center_diatance_x;
    short center_diatance_y;
    void get_center_distance();
    void Search_last(vector<Armor_ptr> &last_armors, int frame_number);
    void updateKlaman();
    bool kalmanInited = false;
    ArmorType type = ArmorType::Small;
    KalmanFilter KF;
    void get_center();
    ResultPnP get_pnpdata(ArmorType type, light_blobs &blob, Mat &cameraMatrix, Mat &distcoeff);
    static double distance(vector<Point> &blob);
    static double distance(Point a, Point b);
    Point2f armor_center;
    Armor_ptr last_armor = nullptr;
    int numberOf_frame;
    double next_area = 0;
    double armor_area = 0;
    bool isMatched = false;
    bool isFirstFrame = false;
    bool hasId = false;
    int id;
    int Track_num;
    float relative_yaw;
    float relative_picth;
    float record_time;
    Point kalman_center;
    short predicted_dist_x; //预测后的相对坐标//
    short predicted_dist_y;

private:
    int kalmantimes = 0;
    float yaw_v;
    float picth_v;
};

#endif